using System;
using System.Diagnostics;
using System.Runtime.InteropServices;
using System.Text;

namespace RSEngine.Math
{

    /// <summary>
    ///	Class encapsulating a standard 4x4 homogenous matrix.
    /// </summary>
    /// <remarks>
    ///	The use of column vectors and right-to-left ordering is the
    ///	standard in most mathematical texts, and is the same as used in
    ///	OpenGL.
    ///	
    /// For programming purposes, OpenGL matrices are 16-value arrays
    /// with base vectors laid out contiguously in memory.
    ///	
    ///	The generic form of the matrix:
    ///	
    ///	| m[0][0]  m[1][0]  m[2][0]  m[3][0] |
    ///	| m[0][1]  m[1][1]  m[2][1]  m[3][1] |
    ///	| m[0][2]  m[1][2]  m[2][2]  m[3][2] |
    ///	| m[0][3]  m[1][3]  m[1][3]  m[3][3] |
    ///	
    /// Matrix theory :
    /// http://en.wikipedia.org/wiki/Transformation_matrix#Composing_and_inverting_transformations
    ///	</remarks>
    [StructLayout(LayoutKind.Sequential)]
    public struct Matrix4
    {

        #region Variables

        public float[] Values;

        public readonly static Matrix4 Zero = new Matrix4(
            0, 0, 0, 0,
            0, 0, 0, 0,
            0, 0, 0, 0,
            0, 0, 0, 0);

        public readonly static Matrix4 Identity = new Matrix4(
            1, 0, 0, 0,
            0, 1, 0, 0,
            0, 0, 1, 0,
            0, 0, 0, 1);

        #endregion

        #region Constructors

        /// <summary>
        ///	Creates a new Matrix4 with all the specified parameters.
        /// </summary>
        public Matrix4(float m00, float m10, float m20, float m30,
            float m01, float m11, float m21, float m31,
            float m02, float m12, float m22, float m32,
            float m03, float m13, float m23, float m33)
        {
            Values = new float[4 * 4];

            this.M00 = m00;
            this.M01 = m01;
            this.M02 = m02;
            this.M03 = m03;
            this.M10 = m10;
            this.M11 = m11;
            this.M12 = m12;
            this.M13 = m13;
            this.M20 = m20;
            this.M21 = m21;
            this.M22 = m22;
            this.M23 = m23;
            this.M30 = m30;
            this.M31 = m31;
            this.M32 = m32;
            this.M33 = m33;
        }

        /// <summary>
        ///	Creates a new Matrix4 from a values array.
        /// </summary>
        public Matrix4(float[] values)
        {
            Values = values;
        }

        #endregion

        #region properties

        /// <summary>
        ///		Gets/Sets the Translation portion of the matrix.
        ///		| 0 0 0 Tx|
        ///		| 0 0 0 Ty|
        ///		| 0 0 0 Tz|
        ///		| 0 0 0  1 |
        /// </summary>
        public Vector3 Translation
        {
            get
            {
                return new Vector3(this.M03, this.M13, this.M23);
            }
            set
            {
                this.M03 = value.x;
                this.M13 = value.y;
                this.M23 = value.z;
            }
        }

        /// <summary>
        ///		Gets/Sets the Translation portion of the matrix.
        ///		|Sx 0  0  0 |
        ///		| 0 Sy 0  0 |
        ///		| 0  0 Sz 0 |
        ///		| 0  0  0  0 |
        /// </summary>
        public Vector3 Scale
        {
            get
            {
                return new Vector3(this.M00, this.M11, this.M22);
            }
            set
            {
                this.M00 = value.x;
                this.M11 = value.y;
                this.M22 = value.z;
            }
        }

        /// <summary>
        /// Check whether or not the matrix is affine matrix.
        /// </summary>
        /// <remarks>
        /// An affine matrix is a 4x4 matrix with row 3 equal to (0, 0, 0, 1),
        /// e.g. no projective coefficients.
        /// </remarks>
        public bool IsAffine
        {
            get
            {
                return M30 == 0 && M31 == 0 && M32 == 0 && M33 == 1;
            }
        }

        /// <summary>
        /// Check if it is an identity matrix.
        /// </summary>
        public bool IsIdentity
        {
            get
            {
                bool isNotIdentity =
                    (Values[0] != 1) || (Values[1] != 0) || (Values[2] != 0) || (Values[3] != 0) ||
                    (Values[4] != 0) || (Values[5] != 1) || (Values[6] != 0) || (Values[7] != 0) ||
                    (Values[8] != 0) || (Values[9] != 0) || (Values[10] != 1) || (Values[11] != 0) ||
                    (Values[12] != 0) || (Values[13] != 0) || (Values[14] != 0) || (Values[15] != 1);

                return !isNotIdentity;
            }
        }

        #endregion

        #region properties : Myx


        public float M00
        {
            get
            {
                return Values[0];
            }
            set
            {
                Values[0] = value;
            }
        }

        public float M10
        {
            get
            {
                return Values[1];
            }
            set
            {
                Values[1] = value;
            }
        }

        public float M20
        {
            get
            {
                return Values[2];
            }
            set
            {
                Values[2] = value;
            }
        }

        public float M30
        {
            get
            {
                return Values[3];
            }
            set
            {
                Values[3] = value;
            }
        }

        public float M01
        {
            get
            {
                return Values[4];
            }
            set
            {
                Values[4] = value;
            }
        }

        public float M11
        {
            get
            {
                return Values[5];
            }
            set
            {
                Values[5] = value;
            }
        }

        public float M21
        {
            get
            {
                return Values[6];
            }
            set
            {
                Values[6] = value;
            }
        }

        public float M31
        {
            get
            {
                return Values[7];
            }
            set
            {
                Values[7] = value;
            }
        }

        public float M02
        {
            get
            {
                return Values[8];
            }
            set
            {
                Values[8] = value;
            }
        }

        public float M12
        {
            get
            {
                return Values[9];
            }
            set
            {
                Values[9] = value;
            }
        }

        public float M22
        {
            get
            {
                return Values[10];
            }
            set
            {
                Values[10] = value;
            }
        }

        public float M32
        {
            get
            {
                return Values[11];
            }
            set
            {
                Values[11] = value;
            }
        }

        public float M03
        {
            get
            {
                return Values[12];
            }
            set
            {
                Values[12] = value;
            }
        }

        public float M13
        {
            get
            {
                return Values[13];
            }
            set
            {
                Values[13] = value;
            }
        }

        public float M23
        {
            get
            {
                return Values[14];
            }
            set
            {
                Values[14] = value;
            }
        }

        public float M33
        {
            get
            {
                return Values[15];
            }
            set
            {
                Values[15] = value;
            }
        }

        #endregion

        #region Conversions to/from Matrix3

        /// <summary>
        /// Returns a 3x3 portion of this 4x4 matrix.
        /// </summary>
        public Matrix3 ToMatrix3()
        {
            return
                new Matrix3(
                    this.M00, this.M01, this.M02,
                    this.M10, this.M11, this.M12,
                    this.M20, this.M21, this.M22);
        }

        /// <summary>
        ///	Used to allow assignment from a Matrix3 to a Matrix4 object.
        /// </summary>
        public static Matrix4 FromMatrix3(Matrix3 right)
        {
            return right;
        }

        #endregion

        #region Inverse, InverseAffine, Transpose, TransformAffine

        /// <summary>
        /// Returns an inverted 4d matrix.
        /// </summary>
        public Matrix4 Inverse()
        {
            return Adjoint * (1.0f / this.Determinant);
        }

        /// <summary>
        /// returns an inverted affine Matrix
        /// </summary>
        /// <returns></returns>
        public Matrix4 InverseAffine()
        {
            Debug.Assert(IsAffine);

            float t00 = M22 * M11 - M21 * M12;
            float t10 = M20 * M12 - M22 * M10;
            float t20 = M21 * M10 - M20 * M11;

            float invDet = 1 / (M00 * t00 + M01 * t10 + M02 * t20);

            t00 *= invDet;
            t10 *= invDet;
            t20 *= invDet;

            M00 *= invDet;
            M01 *= invDet;
            M02 *= invDet;

            float r00 = t00;
            float r01 = M02 * M21 - M01 * M22;
            float r02 = M01 * M12 - M02 * M11;

            float r10 = t10;
            float r11 = M00 * M22 - M02 * M20;
            float r12 = M02 * M10 - M00 * M12;

            float r20 = t20;
            float r21 = M01 * M20 - M00 * M21;
            float r22 = M00 * M11 - M01 * M10;

            float r03 = -(r00 * M03 + r01 * M13 + r02 * M23);
            float r13 = -(r10 * M03 + r11 * M13 + r12 * M23);
            float r23 = -(r20 * M03 + r21 * M13 + r22 * M23);

            return new Matrix4(
                r00, r10, r20, 0,
                r01, r11, r21, 0,
                r02, r12, r22, 0,
                r03, r13, r23, 1);

        }

        /// <summary>
        /// Swap the rows of the matrix with the columns.
        /// </summary>
        /// <returns>A transposed Matrix.</returns>
        public Matrix4 Transpose()
        {
            return new Matrix4(this.M00, this.M01, this.M02, this.M03,
                this.M10, this.M11, this.M12, this.M13,
                this.M20, this.M21, this.M22, this.M23,
                this.M30, this.M31, this.M32, this.M33);
        }

        /// <summary>
        /// 3-D Vector transformation specially for affine matrix.
        /// </summary>
        /// <remarks>
        /// Transforms the given 3-D vector by the matrix, projecting the
        /// result back into <i>w</i> = 1.
        /// The matrix must be an affine matrix. <see cref="Matrix4.IsAffine"/>.
        /// </remarks>
        public Vector3 TransformAffine(Vector3 v)
        {
            Debug.Assert(IsAffine);

            return new Vector3(
                    M00 * v.x + M01 * v.y + M02 * v.z + M03,
                    M10 * v.x + M11 * v.y + M12 * v.z + M13,
                    M20 * v.x + M21 * v.y + M22 * v.z + M23);
        }

        /// <summary>
        /// 4-D Vector transformation specially for affine matrix.
        /// </summary>
        /// <remarks>
        /// The matrix must be an affine matrix. <see cref="Matrix4.IsAffine"/>.
        /// </remarks>
        public Vector4 TransformAffine(Vector4 v)
        {
            Debug.Assert(IsAffine);

            return new Vector4(
                M00 * v.x + M01 * v.y + M02 * v.z + M03 * v.w,
                M10 * v.x + M11 * v.y + M12 * v.z + M13 * v.w,
                M20 * v.x + M21 * v.y + M22 * v.z + M23 * v.w,
                v.w);
        }

        #endregion

        #region Operator overloads

        /// <summary>
        ///	Used to multiply (concatenate) two 4x4 Matrices.
        /// </summary>
        /// <param name="left"></param>
        /// <param name="right"></param>
        /// <returns></returns>
        public static Matrix4 operator *(Matrix4 left, Matrix4 right)
        {
            Matrix4 result = Matrix4.Zero.Clone();

            result.M00 = left.M00 * right.M00 + left.M01 * right.M10 + left.M02 * right.M20 + left.M03 * right.M30;
            result.M01 = left.M00 * right.M01 + left.M01 * right.M11 + left.M02 * right.M21 + left.M03 * right.M31;
            result.M02 = left.M00 * right.M02 + left.M01 * right.M12 + left.M02 * right.M22 + left.M03 * right.M32;
            result.M03 = left.M00 * right.M03 + left.M01 * right.M13 + left.M02 * right.M23 + left.M03 * right.M33;

            result.M10 = left.M10 * right.M00 + left.M11 * right.M10 + left.M12 * right.M20 + left.M13 * right.M30;
            result.M11 = left.M10 * right.M01 + left.M11 * right.M11 + left.M12 * right.M21 + left.M13 * right.M31;
            result.M12 = left.M10 * right.M02 + left.M11 * right.M12 + left.M12 * right.M22 + left.M13 * right.M32;
            result.M13 = left.M10 * right.M03 + left.M11 * right.M13 + left.M12 * right.M23 + left.M13 * right.M33;

            result.M20 = left.M20 * right.M00 + left.M21 * right.M10 + left.M22 * right.M20 + left.M23 * right.M30;
            result.M21 = left.M20 * right.M01 + left.M21 * right.M11 + left.M22 * right.M21 + left.M23 * right.M31;
            result.M22 = left.M20 * right.M02 + left.M21 * right.M12 + left.M22 * right.M22 + left.M23 * right.M32;
            result.M23 = left.M20 * right.M03 + left.M21 * right.M13 + left.M22 * right.M23 + left.M23 * right.M33;

            result.M30 = left.M30 * right.M00 + left.M31 * right.M10 + left.M32 * right.M20 + left.M33 * right.M30;
            result.M31 = left.M30 * right.M01 + left.M31 * right.M11 + left.M32 * right.M21 + left.M33 * right.M31;
            result.M32 = left.M30 * right.M02 + left.M31 * right.M12 + left.M32 * right.M22 + left.M33 * right.M32;
            result.M33 = left.M30 * right.M03 + left.M31 * right.M13 + left.M32 * right.M23 + left.M33 * right.M33;

            return result;
        }

        /// <summary>
        ///	Transforms a plane using the specified transform.
        /// </summary>
        /// <param name="matrix">Transformation matrix.</param>
        /// <param name="plane">Plane to transform.</param>
        /// <returns>A transformed plane.</returns>
        public static Plane Multiply(Matrix4 matrix, Plane plane)
        {
            return matrix * plane;
        }

        /// <summary>
        ///	Transforms the given 3-D vector by the matrix, projecting the 
        ///	result back into <i>w</i> = 1.
        ///	
        ///	This means that the initial <i>w</i> is considered to be 1.0,
        ///	and then all the tree elements of the resulting 3-D vector are
        ///	divided by the resulting <i>w</i>.
        /// </summary>
        /// <param name="matrix">A Matrix4.</param>
        /// <param name="vector">A Vector3.</param>
        /// <returns>A new vector.</returns>
        public static Vector3 operator *(Matrix4 matrix, Vector3 vector)
        {
            Vector3 result = new Vector3();

            float inverseW = 1.0f / (matrix.M30 + matrix.M31 + matrix.M32 + matrix.M33);

            result.x = ((matrix.M00 * vector.x) + (matrix.M01 * vector.y) + (matrix.M02 * vector.z) + matrix.M03) * inverseW;
            result.y = ((matrix.M10 * vector.x) + (matrix.M11 * vector.y) + (matrix.M12 * vector.z) + matrix.M13) * inverseW;
            result.z = ((matrix.M20 * vector.x) + (matrix.M21 * vector.y) + (matrix.M22 * vector.z) + matrix.M23) * inverseW;

            return result;
        }

        /// <summary>
        /// Transforms the given 3-D ray by the matrix
        /// </summary>
        public static Ray operator *(Matrix4 matrix, Ray ray)
        {
            Ray result = new Ray();

            // With the translation
            result.Origin.x = (matrix.M00 * ray.Origin.x) + (matrix.M01 * ray.Origin.y) + (matrix.M02 * ray.Origin.z) + matrix.M03;
            result.Origin.y = (matrix.M10 * ray.Origin.x) + (matrix.M11 * ray.Origin.y) + (matrix.M12 * ray.Origin.z) + matrix.M13;
            result.Origin.z = (matrix.M20 * ray.Origin.x) + (matrix.M21 * ray.Origin.y) + (matrix.M22 * ray.Origin.z) + matrix.M23;

            // No translation
            result.Direction.x = (matrix.M00 * ray.Direction.x) + (matrix.M01 * ray.Direction.y) + (matrix.M02 * ray.Direction.z);
            result.Direction.y = (matrix.M10 * ray.Direction.x) + (matrix.M11 * ray.Direction.y) + (matrix.M12 * ray.Direction.z);
            result.Direction.z = (matrix.M20 * ray.Direction.x) + (matrix.M21 * ray.Direction.y) + (matrix.M22 * ray.Direction.z);

            return result;
        }

        /// <summary>
        ///	Used to multiply a Matrix4 object by a scalar value..
        /// </summary>
        public static Matrix4 operator *(Matrix4 left, float scalar)
        {
            Matrix4 result = Matrix4.Zero.Clone();

            result.M00 = left.M00 * scalar;
            result.M01 = left.M01 * scalar;
            result.M02 = left.M02 * scalar;
            result.M03 = left.M03 * scalar;

            result.M10 = left.M10 * scalar;
            result.M11 = left.M11 * scalar;
            result.M12 = left.M12 * scalar;
            result.M13 = left.M13 * scalar;

            result.M20 = left.M20 * scalar;
            result.M21 = left.M21 * scalar;
            result.M22 = left.M22 * scalar;
            result.M23 = left.M23 * scalar;

            result.M30 = left.M30 * scalar;
            result.M31 = left.M31 * scalar;
            result.M32 = left.M32 * scalar;
            result.M33 = left.M33 * scalar;

            return result;
        }

        /// <summary>
        ///	Used to multiply a transformation to a Plane.
        /// </summary>
        public static Plane operator *(Matrix4 left, Plane plane)
        {
            Plane result = new Plane();

            Vector3 planeNormal = plane.Normal;

            result.Normal = new Vector3(
                left.M00 * planeNormal.x + left.M01 * planeNormal.y + left.M02 * planeNormal.z,
                left.M10 * planeNormal.x + left.M11 * planeNormal.y + left.M12 * planeNormal.z,
                left.M20 * planeNormal.x + left.M21 * planeNormal.y + left.M22 * planeNormal.z);

            Vector3 pt = planeNormal * -plane.D;
            pt = left * pt;

            result.D = -pt.Dot(result.Normal);

            return result;
        }

        /// <summary>
        ///	Used to add two matrices together.
        /// </summary>
        /// <param name="left"></param>
        /// <param name="right"></param>
        /// <returns></returns>
        public static Matrix4 Add(Matrix4 left, Matrix4 right)
        {
            return left + right;
        }

        /// <summary>
        ///	Used to add two matrices together.
        /// </summary>
        public static Matrix4 operator +(Matrix4 left, Matrix4 right)
        {
            Matrix4 result = Matrix4.Zero.Clone();

            result.M00 = left.M00 + right.M00;
            result.M01 = left.M01 + right.M01;
            result.M02 = left.M02 + right.M02;
            result.M03 = left.M03 + right.M03;

            result.M10 = left.M10 + right.M10;
            result.M11 = left.M11 + right.M11;
            result.M12 = left.M12 + right.M12;
            result.M13 = left.M13 + right.M13;

            result.M20 = left.M20 + right.M20;
            result.M21 = left.M21 + right.M21;
            result.M22 = left.M22 + right.M22;
            result.M23 = left.M23 + right.M23;

            result.M30 = left.M30 + right.M30;
            result.M31 = left.M31 + right.M31;
            result.M32 = left.M32 + right.M32;
            result.M33 = left.M33 + right.M33;

            return result;
        }

        /// <summary>
        ///	Used to subtract two matrices.
        /// </summary>
        public static Matrix4 operator -(Matrix4 left, Matrix4 right)
        {
            Matrix4 result = Matrix4.Zero.Clone();

            result.M00 = left.M00 - right.M00;
            result.M01 = left.M01 - right.M01;
            result.M02 = left.M02 - right.M02;
            result.M03 = left.M03 - right.M03;

            result.M10 = left.M10 - right.M10;
            result.M11 = left.M11 - right.M11;
            result.M12 = left.M12 - right.M12;
            result.M13 = left.M13 - right.M13;

            result.M20 = left.M20 - right.M20;
            result.M21 = left.M21 - right.M21;
            result.M22 = left.M22 - right.M22;
            result.M23 = left.M23 - right.M23;

            result.M30 = left.M30 - right.M30;
            result.M31 = left.M31 - right.M31;
            result.M32 = left.M32 - right.M32;
            result.M33 = left.M33 - right.M33;

            return result;
        }

        /// <summary>
        /// Compares two Matrix4 instances for equality.
        /// </summary>
        /// <returns>true if the Matrix 4 instances are equal, false otherwise.</returns>
        public static bool operator ==(Matrix4 left, Matrix4 right)
        {
            if (
                left.M00 == right.M00 && left.M01 == right.M01 && left.M02 == right.M02 && left.M03 == right.M03 &&
                left.M10 == right.M10 && left.M11 == right.M11 && left.M12 == right.M12 && left.M13 == right.M13 &&
                left.M20 == right.M20 && left.M21 == right.M21 && left.M22 == right.M22 && left.M23 == right.M23 &&
                left.M30 == right.M30 && left.M31 == right.M31 && left.M32 == right.M32 && left.M33 == right.M33)
                return true;

            return false;
        }

        /// <summary>
        /// Compares two Matrix4 instances for inequality.
        /// </summary>
        /// <returns>true if the Matrix 4 instances are not equal, false otherwise.</returns>
        public static bool operator !=(Matrix4 left, Matrix4 right)
        {
            return !(left == right);
        }

        /// <summary>
        ///	Used to allow assignment from a Matrix3 to a Matrix4 object.
        /// </summary>
        public static implicit operator Matrix4(Matrix3 right)
        {
            Matrix4 result = Matrix4.Identity.Clone();

            result.M00 = right.M00;
            result.M01 = right.M01;
            result.M02 = right.M02;
            result.M10 = right.M10;
            result.M11 = right.M11;
            result.M12 = right.M12;
            result.M20 = right.M20;
            result.M21 = right.M21;
            result.M22 = right.M22;

            return result;
        }

        ///// <summary>
        ///// Allows the Matrix to be accessed like a 2d array (i.e. matrix[2,3])
        ///// </summary>
        ///// <remarks>
        ///// This indexer is only provided as a convenience, and is <b>not</b> recommended for use in
        ///// intensive applications.  
        ///// </remarks>
        public float this[int row, int col]
        {
            get
            {
                unsafe
                {
                    fixed (float* pM = &Values[0])
                        return *(pM + ((4 * row) + col));
                }
            }
            set
            {
                unsafe
                {
                    fixed (float* pM = &Values[0])
                        *(pM + ((4 * row) + col)) = value;
                }
            }
        }

        #endregion

        #region Determinant / Adjoint

        /// <summary>
        /// Gets the determinant of this matrix.
        /// </summary>
        public float Determinant
        {
            get
            {
                // note: this is an expanded version of the Ogre determinant() method, to give better performance in C#. Generated using a script
                float result = M00 * (M11 * (M22 * M33 - M32 * M23) - M12 * (M21 * M33 - M31 * M23) + M13 * (M21 * M32 - M31 * M22)) -
                    M01 * (M10 * (M22 * M33 - M32 * M23) - M12 * (M20 * M33 - M30 * M23) + M13 * (M20 * M32 - M30 * M22)) +
                    M02 * (M10 * (M21 * M33 - M31 * M23) - M11 * (M20 * M33 - M30 * M23) + M13 * (M20 * M31 - M30 * M21)) -
                    M03 * (M10 * (M21 * M32 - M31 * M22) - M11 * (M20 * M32 - M30 * M22) + M12 * (M20 * M31 - M30 * M21));

                return result;
            }
        }

        /// <summary>
        /// Used to generate the adjoint of this matrix.
        /// </summary>
        /// <returns>The adjoint matrix of the current instance.</returns>
        public Matrix4 Adjoint
        {
            get
            {
                // note: this is an expanded version of the Ogre adjoint() method, to give better performance in C#. Generated using a script
                float val0 = M11 * (M22 * M33 - M32 * M23) - M12 * (M21 * M33 - M31 * M23) + M13 * (M21 * M32 - M31 * M22);
                float val1 = -(M01 * (M22 * M33 - M32 * M23) - M02 * (M21 * M33 - M31 * M23) + M03 * (M21 * M32 - M31 * M22));
                float val2 = M01 * (M12 * M33 - M32 * M13) - M02 * (M11 * M33 - M31 * M13) + M03 * (M11 * M32 - M31 * M12);
                float val3 = -(M01 * (M12 * M23 - M22 * M13) - M02 * (M11 * M23 - M21 * M13) + M03 * (M11 * M22 - M21 * M12));
                float val4 = -(M10 * (M22 * M33 - M32 * M23) - M12 * (M20 * M33 - M30 * M23) + M13 * (M20 * M32 - M30 * M22));
                float val5 = M00 * (M22 * M33 - M32 * M23) - M02 * (M20 * M33 - M30 * M23) + M03 * (M20 * M32 - M30 * M22);
                float val6 = -(M00 * (M12 * M33 - M32 * M13) - M02 * (M10 * M33 - M30 * M13) + M03 * (M10 * M32 - M30 * M12));
                float val7 = M00 * (M12 * M23 - M22 * M13) - M02 * (M10 * M23 - M20 * M13) + M03 * (M10 * M22 - M20 * M12);
                float val8 = M10 * (M21 * M33 - M31 * M23) - M11 * (M20 * M33 - M30 * M23) + M13 * (M20 * M31 - M30 * M21);
                float val9 = -(M00 * (M21 * M33 - M31 * M23) - M01 * (M20 * M33 - M30 * M23) + M03 * (M20 * M31 - M30 * M21));
                float val10 = M00 * (M11 * M33 - M31 * M13) - M01 * (M10 * M33 - M30 * M13) + M03 * (M10 * M31 - M30 * M11);
                float val11 = -(M00 * (M11 * M23 - M21 * M13) - M01 * (M10 * M23 - M20 * M13) + M03 * (M10 * M21 - M20 * M11));
                float val12 = -(M10 * (M21 * M32 - M31 * M22) - M11 * (M20 * M32 - M30 * M22) + M12 * (M20 * M31 - M30 * M21));
                float val13 = M00 * (M21 * M32 - M31 * M22) - M01 * (M20 * M32 - M30 * M22) + M02 * (M20 * M31 - M30 * M21);
                float val14 = -(M00 * (M11 * M32 - M31 * M12) - M01 * (M10 * M32 - M30 * M12) + M02 * (M10 * M31 - M30 * M11));
                float val15 = M00 * (M11 * M22 - M21 * M12) - M01 * (M10 * M22 - M20 * M12) + M02 * (M10 * M21 - M20 * M11);

                return new Matrix4(val0, val1, val2, val3, val4, val5, val6, val7, val8, val9, val10, val11, val12, val13, val14, val15).Transpose();
            }
        }

        #endregion

        #region Rotate

        /// <summary>
        /// Apply a transformation around the rotation axis
        /// </summary>
        /// <param name="rotationAxis">The rotation axis.</param>
        /// <param name="radians">The rotation angle in radians.</param>
        public void Rotate(Vector3 rotationAxis, float radians)
        {
            this = this * Quaternion.FromAngleAxis(rotationAxis, radians).ToRotationMatrix();
        }

        #endregion

        #region Clone

        public Matrix4 Clone()
        {
            Matrix4 clone = new Matrix4();
            clone.Values = new float[4 * 4];
            clone.Values[0] = Values[0];
            clone.Values[1] = Values[1];
            clone.Values[2] = Values[2];
            clone.Values[3] = Values[3];
            clone.Values[4] = Values[4];
            clone.Values[5] = Values[5];
            clone.Values[6] = Values[6];
            clone.Values[7] = Values[7];
            clone.Values[8] = Values[8];
            clone.Values[9] = Values[9];
            clone.Values[10] = Values[10];
            clone.Values[11] = Values[11];
            clone.Values[12] = Values[12];
            clone.Values[13] = Values[13];
            clone.Values[14] = Values[14];
            clone.Values[15] = Values[15];
            return clone;
        }

        #endregion

        #region Object overloads

        /// <summary>
        ///	Overrides the Object.ToString() method to provide a text representation of 
        ///	a Matrix4.
        /// </summary>
        /// <returns>A string representation of a vector3.</returns>
        public override string ToString()
        {
            StringBuilder sb = new StringBuilder();

            sb.AppendFormat(" | {0} {1} {2} {3} |\n", this.M00, this.M01, this.M02, this.M03);
            sb.AppendFormat(" | {0} {1} {2} {3} |\n", this.M10, this.M11, this.M12, this.M13);
            sb.AppendFormat(" | {0} {1} {2} {3} |\n", this.M20, this.M21, this.M22, this.M23);
            sb.AppendFormat(" | {0} {1} {2} {3} |\n", this.M30, this.M31, this.M32, this.M33);

            return sb.ToString();
        }

        /// <summary>
        ///	Provides a unique hash code based on the member variables of this
        ///	class.  This should be done because the equality operators (==, !=)
        ///	have been overriden by this class.
        ///	
        ///	The standard implementation is a simple XOR operation between all local
        ///	member variables.
        /// </summary>
        public override int GetHashCode()
        {
            int hashCode = 0;

            unsafe
            {
                fixed (float* pM = &Values[0])
                {
                    for (int i = 0; i < 16; i++)
                        hashCode ^= (int)(*(pM + i));
                }
            }

            return hashCode;
        }

        /// <summary>
        ///	Compares this Matrix to another object.  This should be done because the 
        ///	equality operators (==, !=) have been overriden by this class.
        /// </summary>
        /// <param name="obj"></param>
        /// <returns></returns>
        public override bool Equals(object obj)
        {
            return obj is Matrix4 && this == (Matrix4)obj;
        }

        #endregion

    }

}
